#include <bwio.h>
#include <io.h>
#include <user.h>
#include <defs.h>
#include <controller.h>
#include <ts7200.h>
#include <string.h>
#include <printer.h>
#include <track.h>
#include <sensor.h>
#include <engine.h>
#include <routefinder.h>
#include <utils.h>
#include <curses.h>

void NameServer();
void ClockServer();
void TrainRxServer();
void TrainTxServer();
void TerminalRxServer();
void TerminalTxServer();
void Controller();
void EngineManager();
void IdleTask();
void Printer();
void RouteFinder();
void SensorManager();
void SwitchManager();
void TrackManger();

void LastSensor(char *external) {
  char sensor_data[10];
  AwaitEvent(EVENT_NEW_SENSOR, sensor_data, 10);
  SensorToExternal(sensor_data, external);
}

int Timer4Read() {
  int *timer4_lval = (int*)( TIMER4_LVAL );
  return *timer4_lval / TIMER4_TICKS_PER_MS;
}

void DataPrint( char *last_sensor, int train_number) {
  int train_speed = GetTrainSpeed(train_number);
  int timestamp = Timer4Read();
  
  Printf("%d %d %s %d\r\n",
      train_number, train_speed, last_sensor, timestamp );
}

void TrainDriver() {
  int i;
  int min = 6, max = 14;

  // get number of train
  int train_number = 0;
  Printf("Train: ");
  int c;
  while (TRUE) {
    c = Getc(COM2);
    if (c == '\r')
      break;
    if (c >= '0' && c <= '9') {
      Putc(COM2, c);
      train_number = train_number*10+(c-'0');
    }
  }
  Printf("\r\nTrain %d selected.\r\n", train_number);

  // init train
  Tr(train_number, 0);

  // init switches
  for (i = 1; i <= 156; i++) {
    if (i == 19)  // skip non-existent switches
      i = 153;
    Sw(i, 'C');
  }
  Sw(8, 'S');
  Sw(9, 'S');
  Sw(14, 'S');
  Sw(15, 'S');

  // init timer 4
  int *timer4_hval = (int*)( TIMER4_HVAL );
  *timer4_hval = T4EN_MASK;

  // message to parent: we're starting!
  Send(0, NULL, 0, NULL, 0);
  char last[5];
  StringCopy(last, "----");

  // Phase 0: the starting gate
  Printf("Phase 0\r\n");
  Tr(train_number, 9);
  while (!StringCompare(last, "E11", 5)) {
    LastSensor(last);
    DataPrint(last, train_number);
  }
  StringCopy(last, "----");
  // Phase 1: N outer loops
  Printf("Phase 1\r\n");
  for (i = min; i < max; i++) {
    Tr(train_number, i);
    // swap 6, 7 for branch at bottom
    Sw(6, GetSwitchState(6) == 'C' ? 'S' : 'C');
    Sw(7, GetSwitchState(7) == 'C' ? 'S' : 'C');
    while (!StringCompare(last, "C5", 5)) {
      LastSensor(last);
      DataPrint(last, train_number);
    }
    StringCopy(last, "----");
  }
  // Phase 2: N figure-eights
  Printf("Phase 2\r\n");
  Sw(8, 'C');
  Sw(9, 'C');
  Sw(14, 'C');
  Sw(15, 'C');
  for (i = min; i < max; i++) {
    Tr(train_number, i);
    while (!StringCompare(last, "D5", 5)) {
      LastSensor(last);
      DataPrint(last, train_number);
    }
    StringCopy(last, "----");
    Sw(153, 'C');
    Sw(154, 'S');
    Sw(155, 'C');
    Sw(156, 'S');
    while (!StringCompare(last, "A3", 5)) {
      LastSensor(last);
      DataPrint(last, train_number);
    }
    StringCopy(last, "----");
    Sw(153, 'S');
    Sw(154, 'C');
    Sw(155, 'S');
    Sw(156, 'C');
  }
  while (!StringCompare(last, "E9", 5)) {
    LastSensor(last);
    DataPrint(last, train_number);
  }
  StringCopy(last, "----");
  // Phase 3: N inner loops
  Printf("Phase 3\r\n");
  Sw(10, 'S');
  Sw(13, 'S');
  Sw(16, 'S');
  Sw(17, 'S');
  for (i = min; i < max; i++) {
    Tr(train_number, i);
    while (!StringCompare(last, "E9", 5)) {
      LastSensor(last);
      DataPrint(last, train_number);
    }
    StringCopy(last, "----");
  }
  // Phase 4: reverse N inner loops
  Printf("Phase 4\r\n");
  Rv(train_number);
  for (i = min; i < max; i++) {
    Tr(train_number, i);
    while (!StringCompare(last, "D5", 5)) {
      LastSensor(last);
      DataPrint(last, train_number);
    }
    StringCopy(last, "----");
  }
  // Phase 5: reverse N figure-eights
  Printf("Phase 5\r\n");
  Sw(10, 'C');
  Sw(13, 'C');
  Sw(16, 'C');
  Sw(17, 'C');
  for (i = min; i < max; i++) {
    Tr(train_number, i);
    while (!StringCompare(last, "E9", 5)) {
      LastSensor(last);
      DataPrint(last, train_number);
    }
    StringCopy(last, "----");
    Sw(153, 'C');
    Sw(154, 'S');
    Sw(155, 'C');
    Sw(156, 'S');
    while (!StringCompare(last, "B15", 5)) {
      LastSensor(last);
      DataPrint(last, train_number);
    }
    StringCopy(last, "----");
    Sw(153, 'S');
    Sw(154, 'C');
    Sw(155, 'S');
    Sw(156, 'C');
  }
  while (!StringCompare(last, "E9", 5)) {
    LastSensor(last);
    DataPrint(last, train_number);
  }
  StringCopy(last, "----");
  // Phase 6: reverse N outer loops
  Printf("Phase 6\r\n");
  Sw(8, 'C');
  Sw(9, 'C');
  Sw(14, 'C');
  Sw(15, 'C');
  for (i = min; i < max; i++) {
    Tr(train_number, i);
    while (!StringCompare(last, "D7", 5)) {
      LastSensor(last);
      DataPrint(last, train_number);
    }
    StringCopy(last, "----");
    // swap 6, 7 for branch at bottom
    Sw(6, GetSwitchState(6) == 'C' ? 'S' : 'C');
    Sw(7, GetSwitchState(7) == 'C' ? 'S' : 'C');
  }
  // Phase 7: enter finishing section
  Printf("Phase 7\r\n");
  Sw(6, 'C');
  Sw(7, 'C');
  Sw(18, 'S');
  while (!StringCompare(last, "A5", 5)) {
    LastSensor(last);
    DataPrint(last, train_number);
  }
  StringCopy(last, "----");
  // Phase 8: STOP!
  Printf("Phase 8\r\n");
  Tr(train_number, 0);
  // message to parent: we're done!
  Send(0, NULL, 0, NULL, 0);
  Exit();
}

void UserInit() {
  int child;
  child = Create(4, &NameServer, "NameServer");
  Send(child, NULL, 0, NULL, 0);
  child = Create(6, &ClockServer, "ClockServer");
  Send(child, NULL, 0, NULL, 0);
  child = Create(7, &Printer, "Printer");
  Send(child, NULL, 0, NULL, 0);
  child = Create(4, &TrainTxServer, "TrainTxServer");
  Send(child, NULL, 0, NULL, 0);
  child = Create(5, &TrainRxServer, "TrainRxServer");
  Send(child, NULL, 0, NULL, 0);
  child = Create(4, &TerminalRxServer, "TerminalRxServer");
  Send(child, NULL, 0, NULL, 0);
  child = Create(5, &TerminalTxServer, "TerminalTxServer");
  Send(child, NULL, 0, NULL, 0);
  child = Create( 6, &Controller, "Controller" );
  Send(child, NULL, 0, NULL, 0);
   
  Create(0, &IdleTask, "IdleTask");
  
  ResetScreen();
  ClearScreen();
  PrintAt(1, 19, "Please enter the track type (A or B): " );
  char c;
  do {
    c = Getc(COM2);
  } while (c != 'A' && c != 'B');

  child = Create( 3, &TrackManager, "TrackManager" );
  Send(child, &c, 1, NULL, 0);
  Printf("1");
  child = Create( 8, &SensorManager, "SensorManager");
  Send(child, NULL, 0, NULL, 0);
  Printf("2");

  child = Create(2, &TrainDriver, "TrainDriver");
  Receive(&child, NULL, 0);
  Reply(child, NULL, 0);
  Receive(&child, NULL, 0);
  Reply(child, NULL, 0);
  FingerOfDeath();
}
